#include "jakamovetask.h"
#define PI 3.1415926

JakaMoveTask::JakaMoveTask() {}

void JakaMoveTask::run(){
    double dblJoint1                = parameters["dblJoint1"].toDouble();
    double dblJoint2                = parameters["dblJoint2"].toDouble();
    double dblJoint3                = parameters["dblJoint3"].toDouble();
    double dblJoint4                = parameters["dblJoint4"].toDouble();
    double dblJoint5                = parameters["dblJoint5"].toDouble();
    double dblJoint6                = parameters["dblJoint6"].toDouble();
    double dblVelocity              = parameters["dblVelocity"].toDouble();
    //double dblAcceleration          = parameters["dblAcceleration"].toDouble();
    m_pRobot                        = (JAKAZuRobot*)(parameters["pRobot"].value<void*>());

    JointValue          jointValue;
    jointValue.jVal[0] = PI * dblJoint1 / 180.0;
    jointValue.jVal[1] = PI * dblJoint2 / 180.0;
    jointValue.jVal[2] = PI * dblJoint3 / 180.0;
    jointValue.jVal[3] = PI * dblJoint4 / 180.0;
    jointValue.jVal[4] = PI * dblJoint5 / 180.0;
    jointValue.jVal[5] = PI * dblJoint6 / 180.0;
    m_pRobot->joint_move(&jointValue,ABS,TRUE,dblVelocity);//velocity : 1 代表速度为 1rad/s; 机械臂关节运动 ，关节速度上限 180deg/s
}
